/*
control.pde -> Input, Output, Devices

ArduPilotMegaJeti Version 1.0 Public Beta
Author:     Uwe Gartmann

Authors of ArduPilotMega:	Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Michael Smith
Thanks to:	Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

External ADC ADS7844 is connected via Serial port 2 (in SPI mode)
TXD2 = MOSI = pin PH1
RXD2 = MISO = pin PH0
XCK2 = SCK = pin PH2
Chip Select pin is PC4 (33)   [PH6 (9)]
We are using the 16 clocks per conversion timming to increase efficiency (fast)
The sampling frequency is 400Hz (Timer2 overflow interrupt)
So if our loop is at 50Hz, our needed sampling freq should be 100Hz, so
we have an 4x oversampling and averaging.

Methods:
	Init() : Initialization of interrupts an Timers (Timer2 overflow interrupt)
	Ch(ch_num) : Return the ADC channel value

HJI - Input definitions.  USB connector assumed to be on the left, Rx and servo
connector pins to the rear.  IMU shield components facing up.  These are board
referenced sensor inputs, not device referenced.

On Ardupilot Mega Hardware, oriented as described above:
Channel 0 : yaw rate, r
Channel 1 : roll rate, p
Channel 2 : pitch rate, q
Channel 3 : x/y gyro temperature
Channel 4 : x acceleration, aX
Channel 5 : y acceleration, aY
Channel 6 : z acceleration, aZ
Channel 7 : Differential pressure sensor port

*/

/// Definitions
#define bit_set(p,m)   ((p) |= ( 1<<m))
#define bit_clear(p,m) ((p) &= ~(1<<m))

/// Includes
#include <ADS7844.h>
#include <BoardIO.h>

extern "C" {
  // AVR LibC Includes
  #include <inttypes.h>
  #include <avr/interrupt.h>
  #include "WConstants.h"
}

/// Declarations
// Commands for reading ADC channels on ADS7844
static const    uint8_t  adc_cmd[9]       = { 0x87, 0xC7, 0x97, 0xD7, 0xA7, 0xE7, 0xB7, 0xF7, 0x00 };
static volatile uint16_t adc_value[8]     = { 0, 0, 0, 0, 0, 0, 0, 0 };
static volatile long     adc_value_tmp[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static volatile uint8_t  adc_cycle        = 0;

float Sensor[8];
float Offset[8];
float Scale[8];

static unsigned char SPI_transfer(unsigned char data)
{
	/* Wait for empty transmit buffer */
	while ( !( UCSR2A & (1<<UDRE2)) );
	/* Put data into buffer, sends the data */
	UDR2 = data;
	/* Wait for data to be received */
	while ( !(UCSR2A & (1<<RXC2)) );
	/* Get and return received data from buffer */
	return UDR2;
}

ISR (TIMER2_OVF_vect)	// 400 Hz interval
{
	uint8_t ch;
	uint16_t adc_tmp //, adc_tmp2;
	
	bit_clear(PORTC,4);								// Enable Chip Select (PIN PC4)
	SPI_transfer(adc_cmd[0]);						// Command to read the first channel
	for (ch=0;ch<8;ch++) {
		adc_tmp = SPI_transfer(0)<<8;				// Read first byte (7 bits), shift 8 bits left; first bit (MSB) is always zero (no data)
		adc_tmp |= SPI_transfer(adc_cmd[ch+1]);		// Read second byte (last 5 bits) and send next command
		adc_value_tmp[ch] += adc_tmp>>3;			// Shift 3 bit to the right
	}
	bit_set(PORTC,4);								// Disable Chip Select (PIN PC4)
	if (adc_cycle == 3) {							// four values collected
		for (ch=0;ch<8;ch++) {
			adc_value[ch] = adc_value_tmp[ch]>>2;	// calculate average, bit shift >>2 => divide to 4 
			adc_value_tmp[ch] = 0;					// clear
		}
		adc_cycle = 0;
	} else {
		adc_cycle++;
	}
	TCNT2 = 104;									// 400 Hz
	//bit_clear(PORTF,6);
}

// Constructors ////////////////////////////////////////////////////////////////
ADS7844::ADS7844() {
}

// Public Methods //////////////////////////////////////////////////////////////
void ADS7844::Init(void) {
	uint8_t tmp;

	pinMode(ADC_CHIP_SELECT,OUTPUT);
	digitalWrite(ADC_CHIP_SELECT,HIGH);                 // Disable device (Chip select is active low)
  
	// Initialise Scale factors
	for (uint8_t ch=0; ch<8; ch++) {
		Scale[ch] = 1.0;
	}

	// Setup Serial Port2 in SPI mode
	UBRR2 = 0;
	DDRH |= (1<<PH2);                                   // SPI clock XCK2 (PH2) as output. This enable SPI Master mode
	// Set MSPI mode of operation and SPI data mode 0.
	UCSR2C = (1<<UMSEL21)|(1<<UMSEL20);                 //|(0<<UCPHA2)|(0<<UCPOL2);
	// Enable receiver and transmitter.
	UCSR2B = (1<<RXEN2)|(1<<TXEN2);
	// Set Baud rate
	UBRR2 = 2;                                          // SPI clock running at 2.6MHz


	// Enable Timer2 Overflow interrupt to capture ADC data
	TIMSK2 = 0;                                         // Disable interrupts
	TCCR2A = 0;                                         // normal counting mode
	TCCR2B = _BV(CS21)|_BV(CS22);                       // Set prescaler of 256
	TCNT2  = 0;
	TIFR2  = _BV(TOV2);                                 // clear pending interrupts;
	TIMSK2 = _BV(TOIE2) ;                               // enable the overflow interrupt
}

// Read one channel value
int ADS7844::Ch(uint8_t ch) {
	cli();                                              // We stop interrupts to read the variables
	int result = adc_value[ch];
	sei();
	return result;
}

void ADS7844::Update(void) {
	/// 140us execution time
	float adc_temp;
	//bit_set(PORTF,6);
	cli();                                              // We stop interrupts to read the variables
	for (uint8_t ch=0; ch<8; ch++) {
		adc_temp = float(adc_value[ch]);
		Sensor[ch] = (adc_temp - Offset[ch]) * Scale[ch];
	}
	sei();
	//bit_clear(PORTF,6);
}

void ADS7844::calibrateACC(void) {
	// position the sensor horizontal before calling this function
	// zero position of z is derivated from x and y axis
	Offset[IMU_accX] = float(adc_value[IMU_accX]);
	Offset[IMU_accY] = float(adc_value[IMU_accY]);
	Offset[IMU_accZ] = (Offset[IMU_accX] + Offset[IMU_accY] + 80) / 2; // 80 = observed offset error from x and y
}

void ADS7844::calibrateGYRO(void) {
	Offset[IMU_roll] = float(adc_value[IMU_roll]);
	Offset[IMU_pitch] = float(adc_value[IMU_pitch]);
	Offset[IMU_yaw] = float(adc_value[IMU_yaw]);
}
